package es.uji.viselab.robot.links;

import java.text.DecimalFormat;

import es.uji.viselab.kinematics.DKinematic;
import es.uji.viselab.math.Joints;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.robot.RealRobot;
import es.uji.viselab.robot.RobotCatalog;
import es.uji.viselab.robot.RobotException;
import es.uji.viselab.robot.links.fanuc.RobotCommunicationFanucARCMate120;

public class RobotFanucARCMate120 extends RealRobot {

	 private DKinematic dKinematic;

		// Pose end effector based on robot coordinate system
		// double[][] pose;

		public RobotFanucARCMate120(RobotCatalog robotModel) {
	        super(robotModel);
			this.robotModel = robotModel;

			// Position of the end effector
			Matrix4 orientation = new Matrix4();
			orientation.setIdentity();
			orientation.rotY(Math.PI);
			Matrix4 translation = new Matrix4();
			translation.set(new Vector3(1.0, 0.0, 1.0));
			pose = new Matrix4();
			pose.mul(translation, orientation);

			// joints = robotModel.getDefaultJointsPosition();
			 dKinematic = new DKinematic(robotModel);
			 
			 // TODO este es el pose del FANUC
			// joints = robotModel.getDefaultJointsPosition();
			//pose = dKinematic.getDirectKinematicMatrix(joints);

		}

		@Override
		public void connect(String ip, int port) throws RobotException {
			super.connect(ip,port);
			if (connected) {
				throw new RobotException("Cannot connect to robot, it is already connected");
			} else {
				connected = true;
			}
		}

		@Override
		public boolean isConnected() {
			return this.connected;
		}

		@Override
		public String ping() throws RobotException {
			return "Pong from Virtual Robot";
		}

		@Override
		public void disconnect() throws RobotException {
			if (connected) {
				connected = false;
			} else {
				throw new RobotException("Cannot disconnect from robot, I am not connected");
			}
		}

		@Override
		public void move(Joints j) throws RobotException {
			
			this.joints=j.getArray();
			//.poseMal.setJoints(j); RobotToolLocation rtl =
			this.pose=dKinematic.getDirectKinematicMatrix(joints);
			//this.pose.m03=pose.getX();
			//this.pose.m13=pose.getY();
			//this.pose.m23=pose.getZ();
			//		; poseMal.setRtl(rtl);
			notifyPositionChange(this);
			
		}


		@Override
		public String getName() {
			return robotModel.getName();
		}

		@Override
		public boolean isVirtual() {
			return true;
		}

		@Override
		public boolean isRobotConnected() {
			// TODO Auto-generated method stub
			return false;
		}

		@Override
		public Joints getJoints() throws RobotException {
			double[] j = robotModel.getDefaultJointsPosition();
			if (j.length==6)
			  return new Joints(j[0], j[1], j[2], j[3], j[4], j[5]);
			if (j.length==0)
				  return null;
		    return null;
		}

		@Override
		public void move(double[] v) throws RobotException {

			Matrix4 translate = new Matrix4();
			translate.set(new Vector3(v[0], v[1], v[2]));
			pose.mul( translate,pose);
			DecimalFormat df = new DecimalFormat("#.######");
			System.out.println("Desp: " + df.format(v[0]) + " " + df.format(v[1]) + " "
					+ df.format(v[2]) + " new: " + df.format(pose.m03) + "  " + df.format(pose.m13)
					+ "  " + df.format(pose.m23));
			notifyPositionChange(this);
		}

		@Override
		public void setPosition(double[] v) throws RobotException {

			double[] displacement = new double[6];
			displacement[0] = pose.m03 - v[0];
			displacement[1] = v[1] - pose.m13 ;
			displacement[2] = pose.m23 - v[2];
			displacement[3] = 0;
			displacement[4] = 0;
			displacement[5] = 0;
			move(displacement);
		}


}